;; some initialize functions
(defun set-robot (&optional (robot :pr2))
  (case robot
    (:pr2
     (unless (boundp '*pr2*)
       (load "package://pr2eus/pr2.l")
       (pr2))
     (setq *robot* *pr2*))
    (:hrp2jsknt
     (unless (boundp '*hrp2-16*)
       (hrp2jsknt-simple "*HRP2-16*")
       (send *hrp2-16* :fix-leg-to-coords
             (make-coords) '(:lleg :rleg))
       (make-hrp2jsknt-ecds->hcds-trans))
     (setq *robot* *hrp2-16*))
    (:hironx
     (unless (boundp '*hironx*)
       (hironx-simple))
     (setq *robot* *hironx*)))
  (objects (list *robot*)))

(defun init-graspplanning-req
  (&key (namespace ""))
  (ros::wait-for-service (format nil "~A/GraspPlanning" namespace))
  (setq *graspplanning-req*
        (instance object_manipulation_msgs::GraspPlanningRequest
                  :init)))

(defun init-set-grasp-parameters-req
  (&key (namespace ""))
  (ros::wait-for-service (format nil "~A/SetGraspParameters" namespace))
  (setq *set-grasp-parameters-req*
        (instance orrosplanning::SetGraspParametersRequest :init)))

(unless (boundp '*pickivew*) (pickview :no-menu t))

(defun setup-roseus-services ()
  (unless (boundp '*graspplanning-req*) (init-graspplanning-req))
  (unless (boundp '*set-grasp-parameters-req*) (init-set-grasp-parameters-req)))

(defun graspplanning-service-call
  (grasparm target &key (namespace ""))
  (send *graspplanning-req* :arm_name
        (if (equal grasparm :larm)
            "leftarm" "rightarm"))
  (send *graspplanning-req* :target target)
  (ros::service-call
   (format nil "~A/GraspPlanning" namespace)
   *graspplanning-req*))

(defun set-grasp-parameters-service-call
  (&key (namespace ""))
  (ros::service-call
   (format nil "~A/SetGraspParameters" namespace)
   *set-grasp-parameters-req*))

(load "package://graspplanning_openrave_eus_interface/euslisp/eusmodel-to-trimesh-ordered-pcloud.l")

(defun create-GraspableObject-msg-from-pcloud
  (3dp frame_id &key (display-3dp t) (index-channel nil))
  (let* (cluster-points ret)
    (setq cluster-points (make-msg-from-3dpointcloud 3dp :frame frame_id))
    (when index-channel
      (send cluster-points :channels
            (cons
             (instance sensor_msgs::ChannelFloat32 :init
                       :name "indices"
                       :values index-channel)
             (send cluster-points :channels)))
      )
    (if display-3dp (objects (list 3dp)))
    (setq ret (instance object_manipulation_msgs::GraspableObject :init
                        :cluster cluster-points))
    ret))

(load "package://humanoid_dualarm_modeling/euslisp/create-obj-sensor-relative-cds.l")

(defun create-approachrays-with-gdome-face (target-3dp &key (level 3))
  (let* ((bb (make-bounding-box (send target-3dp :points)))
         (radius (+ 100 (max (norm (send bb :minpoints)) (norm (send bb :maxpoints)))))
         (face-cds (create-obj->sensor-relative-cds-lst-with-S2
                    :level level
                    :depth radius))
         (approachrays-lst
          (mapcar #'(lambda (x)
                      (concatenate float-vector
                                   (send (car x) :worldpos)
                                   (send (car x) :z-axis)))
                  face-cds)))
    (setq *approachrays* (apply #'concatenate float-vector approachrays-lst))
    )
  )

(defun set-grasp-parameters (3dp &optional (robot :pr2))
  (unless (boundp '*approachrays*)
    (create-approachrays-with-gdome-face)
    (send *set-grasp-parameters-req* :approachrays
          *approachrays*))
  (send *set-grasp-parameters-req* :preshapes
        (case robot
          (:pr2
           (float-vector (deg2rad 90)))
          (:hrp2jsknt
           (map float-vector #'deg2rad
                (concatenate float-vector
                             (float-vector 90 90 0 0 -40 -40)
                             ;; (float-vector 90 90 0 50 -40 -40)
                             (float-vector 90 90 -15 90 -40 -40))))
          (:hironx
           (map float-vector #'deg2rad
                (float-vector 90 60 -90 60)))))
  (send *set-grasp-parameters-req*)
  (send *set-grasp-parameters-req* :standoffs (scale (/ 1.0 1000.0) (float-vector -20))))

;; global coordinates is defined in util-cds.l.
(defun openrave-gcds->eus-ecds (openrave-gcds arm &optional (robot :pr2))
  (let ((openrave-gcds->eus-ecds-trans
         (if (equal arm :larm)
             (send (cadr (assoc robot *l-openrave-gcds->eus-ecds-lst*)) :copy-worldcoords)
           (send (cadr (assoc robot *r-openrave-gcds->eus-ecds-lst*)) :copy-worldcoords))))
    (send (send openrave-gcds :copy-worldcoords)
          :transform
          openrave-gcds->eus-ecds-trans)))

(defun graspplanning-res->preshape-end-coords-lst
  (grasparm &key (debug nil) (robot :pr2))
  (let ((ret (list)))
    (dolist (g (send *graspplanning-res* :grasps))
      (let* ((preshape
              (coerce
               (mapcar #'(lambda (x) (rad2deg x))
                       (coerce (send g :pre_grasp_posture :position) cons))
               float-vector));;dirty...
             (gcds (ros::tf-pose->coords (send g :grasp_pose)))
             (ecds
              (openrave-gcds->eus-ecds
               gcds grasparm robot)))
        (if debug
            (push (list preshape gcds ecds) ret)
          (push (list preshape ecds) ret))))
    (reverse ret)))

(defun display-gcds ()
  (let ((a-lst
         (mapcar #'(lambda (x)
                     (let ((a (make-arrow 50)))
                       (send a :move-to (ros::tf-pose->coords x) :world)))
                 (send-all (send *graspplanning-res* :grasps) :grasp_pose))))
    (objects (list (objects) a-lst))
    a-lst))

;;now only for hrp2jsknt
(defun display-grasp-candidate (arm &optional (robot :hrp2jsknt))
  (let* ((preshape-ecds-lst (graspplanning-res->preshape-end-coords-lst arm :robot robot))
         (openrave->h-cds
          (if (equal arm :larm)
              (if (equal robot :hrp2jsknt)
                  (send *l-ecds->hcds* :copy-worldcoords)
                (make-coords))
            (if (equal robot :hrp2jsknt)
                (send *r-ecds->hcds* :copy-worldcoords)
              (make-coords))))
         h-lst)
    (case robot
      (:hrp2jsknt
       (setq h-lst (mapcar #'(lambda (x)
                               (let ((h (instance hrp3-hand :init :l/r arm)))
                                 (send h :angle-vector (car x))
                                 (send h :transform
                                       (send
                                        (send (cadr x) :copy-worldcoords)
                                        :transform
                                        (send openrave->h-cds :copy-worldcoords)))
                                 h))
                           preshape-ecds-lst)))
      (:pr2
       (dolist (l preshape-ecds-lst)
         (let ((pr2 (pr2))
               (base->arm-trans
                (send (send l :copy-worldcoords)
                      :transform (send openrave->h-cds :copy-worldcoords))))
           (send pr2 :move-to base->arm-trans :world)
           (objects (append (objects) (flatten (send pr2 :l_gripper_palm_link :inheritance))))
           )
         )
       )
      (:hironx
       (dolist (l preshape-ecds-lst)
         (let ((hironx (hironx-simple))
               (base->arm-trans (send (send l :copy-worldcoords) :transform (send openrave->h-cds :copy-worldcoords))))
           (send hironx :move-to base->arm-trans :world)
           (objects (append (objects) ))
           )
         )))
    ;;(objects (list *obj* h-lst))
    ;;(send *viewer* :draw-objects)
    (list preshape-ecds-lst h-lst)))